#include <math.h>
#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <algorithm>
#include <deque>

#include "contreeconstructor3d.h"
#include "disjointset.h"
#include "../math/nrutil.h"

#define DEBUG_VOLVOL

using namespace PBVP;

static int compareInt(const void* p1, const void* p2)
{
	int x1 = *((int *)p1);
	int x2 = *((int *)p2);

	if (x1 < x2) return -1;
	if (x1 > x2) return 1;
	return 0;
}

ConTreeConstructorReg3D::ConTreeConstructorReg3D(Reg3DVolumeData* pData) 
: m_pData(pData)
{
	m_pData->getDimension(m_Dim);
	m_pData->getOrigin(m_Orig);
	m_pData->getSpan(m_Span);
	m_pCPVector = NULL;
	// touched = new char[6*(m_Dim[0]-1)*(m_Dim[1]-1)*(m_Dim[2]-1)];
}

ConTreeConstructorReg3D::~ConTreeConstructorReg3D(void) 
{
	// if(touched != NULL) delete[] touched;
}


void ConTreeConstructorReg3D::calcLUStars() 
{
	printf("compute all critical points in calcLUStars ...\n");
	assert(m_pCPVector);
	//if (m_pCPVector == NULL)
	//{
	//	sortAllCriticalPoints(m_pCPVector);
	//}
	int i, j, k, nv = m_Dim[0]*m_Dim[1]*m_Dim[2];
	int *p_map = new int[nv];
	for (i = 0; i < nv; i++)
	{
		p_map[(*m_pCPVector)[i].id] = i;
	}

	// for each edge
	for (i = 0; i < nv; i++)
	{
		int id = (*m_pCPVector)[i].id;
		int idx[3], nidx[3];
		m_pData->vert2index(id, idx);
		for (j = 0; j < 14; j++)
		{
			for (k = 0; k < 3; k++)
			{
				nidx[k] = idx[k] + Tetra::six_simplex_neighbors[j][k];
			}
			if (isValidIndex(nidx))
			{
				int nid = m_pData->index2vert(nidx);
				if (i < p_map[nid])
				{
					int nj = p_map[nid];
					(*m_pCPVector)[nj].LS --;
					(*m_pCPVector)[i].US --;
				}
			}
		}
		int t1 = (*m_pCPVector)[i].US;
		int t2 = (*m_pCPVector)[i].LS;
	}

	// for each triangle
	// x planes
	for (i = 0; i < m_Dim[0]; i++)
	{
		for (j = 0; j < m_Dim[1]-1; j++)
		{
			for (k = 0; k < m_Dim[2]-1; k++)
			{
				// each rectangle has two triangles
				bool flag = (i == 0 || i == m_Dim[0]-1);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i, j+1, k), m_pData->index2vert(i, j+1, k+1), flag);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i, j, k+1), m_pData->index2vert(i, j+1, k+1), flag);
			}
		}
	}
	// y planes
	for (j = 0; j < m_Dim[1]; j++)
	{
		for (i = 0; i < m_Dim[0]-1; i++)
		{
			for (k = 0; k < m_Dim[2]-1; k++)
			{
				bool flag = (j == 0 || j == m_Dim[1]-1);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i+1, j, k), m_pData->index2vert(i+1, j, k+1), flag);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i, j, k+1), m_pData->index2vert(i+1, j, k+1), flag);
			}
		}
	}
	// z planes
	for (k = 0; k < m_Dim[2]; k++)
	{
		for (i = 0; i < m_Dim[0]-1; i++)
		{
			for (j = 0; j < m_Dim[1]-1; j++)
			{
				bool flag = (k == 0 || k == m_Dim[2]-1);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i+1, j, k), m_pData->index2vert(i+1, j+1, k), flag);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i, j+1, k), m_pData->index2vert(i+1, j+1, k), flag);
			}
		}
	}
	// inside each cell
	for (i = 0; i < m_Dim[0]-1; i++)
	{
		for (j = 0; j < m_Dim[1]-1; j++)
		{
			for (k = 0; k < m_Dim[2]-1; k++)
			{
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i, j, k+1), m_pData->index2vert(i+1, j+1, k+1), false);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i+1, j+1, k), m_pData->index2vert(i+1, j+1, k+1), false);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i, j+1, k), m_pData->index2vert(i+1, j+1, k+1), false);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i+1, j, k+1), m_pData->index2vert(i+1, j+1, k+1), false);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i+1, j, k), m_pData->index2vert(i+1, j+1, k+1), false);
				LUTriangle(m_pCPVector, p_map, m_pData->index2vert(i, j, k), 
					m_pData->index2vert(i, j+1, k+1), m_pData->index2vert(i+1, j+1, k+1), false);
			}
		}
	}
	// for each tetrahedron
	for (i = 0; i < m_Dim[0]-1; i++)
	{
		for (j = 0; j < m_Dim[1]-1; j++)
		{
			for (k = 0; k < m_Dim[2]-1; k++)
			{
				LUTetrahedron(m_pCPVector, p_map, m_pData->index2vert(i, j, k), m_pData->index2vert(i+1, j, k), 
					m_pData->index2vert(i+1, j, k+1), m_pData->index2vert(i+1, j+1, k+1));
				LUTetrahedron(m_pCPVector, p_map, m_pData->index2vert(i, j, k), m_pData->index2vert(i+1, j, k),
							  m_pData->index2vert(i+1, j+1, k), m_pData->index2vert(i+1, j+1, k+1));
				LUTetrahedron(m_pCPVector, p_map, m_pData->index2vert(i, j, k), m_pData->index2vert(i, j+1, k),
							  m_pData->index2vert(i+1, j+1, k), m_pData->index2vert(i+1, j+1, k+1));
				LUTetrahedron(m_pCPVector, p_map, m_pData->index2vert(i, j, k), m_pData->index2vert(i, j+1, k),
							  m_pData->index2vert(i, j+1, k+1), m_pData->index2vert(i+1, j+1, k+1));
				LUTetrahedron(m_pCPVector, p_map, m_pData->index2vert(i, j, k), m_pData->index2vert(i, j, k+1),
							  m_pData->index2vert(i+1, j, k+1), m_pData->index2vert(i+1, j+1, k+1));
				LUTetrahedron(m_pCPVector, p_map, m_pData->index2vert(i, j, k), m_pData->index2vert(i, j, k+1),
							  m_pData->index2vert(i, j+1, k+1), m_pData->index2vert(i+1, j+1, k+1));
			}
		}
	}
	delete[] p_map;
}

bool ConTreeConstructorReg3D::isMinimum(const CriticalPoint& cp) {
	int idx[3], nidx[3];
	m_pData->vert2index(cp.id, idx);
	for (int j = 0; j < 14; j++)
	{
		for (int k = 0; k < 3; k++)	nidx[k] = idx[k] + Tetra::six_simplex_neighbors[j][k];
		if (isValidIndex(nidx))
		{
			int nid = m_pData->index2vert(nidx);
			CriticalPoint ncp(nid, getCPValue(nid));
			if (ncp < cp) return false;
		}
	}
	return true;
}

bool ConTreeConstructorReg3D::isMaximum(const CriticalPoint& cp) {
	int idx[3], nidx[3];
	m_pData->vert2index(cp.id, idx);
	for (int j = 0; j < 14; j++)
	{
		for (int k = 0; k < 3; k++)	nidx[k] = idx[k] + Tetra::six_simplex_neighbors[j][k];
		if (isValidIndex(nidx))
		{
			int nid = m_pData->index2vert(nidx);
			CriticalPoint ncp(nid, getCPValue(nid));
			if (cp < ncp) return false;
		}
	}
	return true;
}

int ConTreeConstructorReg3D::getNeighbors(int vid, int* neighbors)
{
	int nNeighbors = 0;
	// At most 14 neighbors for a vertex
	int idx3[3], nidx3[3];  

	m_pData->vert2index(vid, idx3);
	for (int j = 0; j < 14; j++)
	{
		for (int k = 0; k < 3; k++)
			nidx3[k] = idx3[k] + Tetra::six_simplex_neighbors[j][k];
		if (isValidIndex(nidx3))
		{
			int nid = m_pData->index2vert(nidx3);
			neighbors[nNeighbors++] = nid;
		}
	}
	return nNeighbors;
}


void ConTreeConstructorReg3D::orderTriangleVerts(int* p_map, int v1, int v2, int v3, int ordered[3])
{
	ordered[0] = p_map[v1];
	ordered[1] = p_map[v2];
	ordered[2] = p_map[v3];
	qsort(ordered, 3, sizeof(int), compareInt);
}

void ConTreeConstructorReg3D::orderTetrahedronVerts(int *p_map, int v1, int v2, int v3, int v4, int ordered[4])
{
	ordered[0] = p_map[v1];
	ordered[1] = p_map[v2];
	ordered[2] = p_map[v3];
	ordered[3] = p_map[v4];
	qsort(ordered, 4, sizeof(int), compareInt);
}

void ConTreeConstructorReg3D::LUTriangle(std::vector<CriticalPoint>* p_v, int *p_map, int v1, int v2, int v3, bool bounflag)
{
	int tri[3];
	orderTriangleVerts(p_map, v1, v2, v3, tri);
	(*p_v)[tri[2]].LS ++;
	(*p_v)[tri[0]].US ++;
	// boundary triangle
	if (bounflag)
	{
		(*p_v)[tri[2]].dbe --;
		(*p_v)[tri[0]].dbe ++;
	}
}

void ConTreeConstructorReg3D::LUTetrahedron(std::vector<CriticalPoint>* p_v, int *p_map, int v1, int v2, int v3, int v4)
{
	int tetra[4];
	orderTetrahedronVerts(p_map, v1, v2, v3, v4, tetra);
	(*p_v)[tetra[3]].LS --;
	(*p_v)[tetra[0]].US --;
}
